#include <stdlib.h>
#include "MegabriteController.h"
#include "WProgram.h"

MegabriteController* newMegabriteController() {
	MegabriteController* megabriteController;
	megabriteController = (MegabriteController*) malloc(sizeof(MegabriteController));
	megabriteController->clockpin = 13;
	megabriteController->enablepin = 10;
	megabriteController->latchpin = 9;
	megabriteController->datapin = 11;
	return megabriteController;
}

void MegabriteController::init() {
	pinMode(datapin, OUTPUT);
	pinMode(latchpin, OUTPUT);
	pinMode(enablepin, OUTPUT);
	pinMode(clockpin, OUTPUT);
	SPCR = (1<<SPE)|(1<<MSTR)|(0<<SPR1)|(1<<SPR0);
	digitalWrite(latchpin, LOW);
	digitalWrite(enablepin, LOW);
}

void MegabriteController::setNumberOfMegabrites(int _numberOfMegabrites) {
	numberOfMegabrites = _numberOfMegabrites;
	int sizeOfMegabrite = sizeof(Megabrite);
	int newArraySize = numberOfMegabrites * sizeOfMegabrite;
	megabrites = (Megabrite**) malloc(newArraySize);
	for (int i = 0; i < _numberOfMegabrites; i++) {
		Megabrite* megabrite = newMegabrite();
		megabrites[i] = megabrite;
	}
}

Megabrite* MegabriteController::getMegabriteAtIndex(int index) {
	if (index >= numberOfMegabrites || index < 0) {
		return 0;
	} else {
		return megabrites[index];
	}
}

void MegabriteController::SB_SendPacket() {
	if (SB_CommandMode == B01) {
		SB_RedCommand = 120;
		SB_GreenCommand = 100;
		SB_BlueCommand = 100;
	}

	SPDR = SB_CommandMode << 6 | SB_BlueCommand>>4;
	while(!(SPSR & (1<<SPIF)));
	SPDR = SB_BlueCommand<<4 | SB_RedCommand>>6;
	while(!(SPSR & (1<<SPIF)));
	SPDR = SB_RedCommand << 2 | SB_GreenCommand>>8;
	while(!(SPSR & (1<<SPIF)));
	SPDR = SB_GreenCommand;
	while(!(SPSR & (1<<SPIF)));
}

void MegabriteController::send() {
	SB_CommandMode = B00; // Write to PWM control registers

	for (int i = 0; i < numberOfMegabrites; i++) {
		Megabrite* megabrite = megabrites[i];
		SB_RedCommand = limit(megabrite->red);
		SB_GreenCommand = limit(megabrite->green);
		SB_BlueCommand = limit(megabrite->blue);
		SB_SendPacket();
	}
 
	delayMicroseconds(1);
 
	PORTB |= (1 << 1);
	delayMicroseconds(1);
	PORTB |= (1 << 2);
 
	delayMicroseconds(1);
 
	PORTB &= ~(1 << 2);
	delayMicroseconds(1);
	PORTB &= ~(1 << 1);
 
 
	SB_CommandMode = B01; // Write to current control registers
	for (int i = 0; i < numberOfMegabrites; i++) {
		SB_SendPacket();
	}
	delayMicroseconds(1);
	PORTB |= (1 << 1);
	delayMicroseconds(1);
	PORTB &= ~(1 << 1);
}

int MegabriteController::limit(int val) {
	if (val > 1023) {
		return 1023;
	} else if (val < 0) {
		return 0;
	}
	return val;	
}

